using System;
using System.Collections.Generic;
using System.IO.Ports;

namespace armcontrolgui.RandomLogicArm
{
	public class RLArm : ArmController
	{
		#region Delclarations
		
		public override event ArmDelegate PositionUpdated;
		
		private List<Axis> ls_Axis;
		private List<Gtk.Widget> ls_View;
		
		private SerialPort sepo_ComPort;
		
		private PortSetting pswin_PortSettingWindow;
		private AddInfo adi_AdditionalInfoWidget;
		
		private Axis ax_Base;
		private Axis ax_Shoulder;
		private Axis ax_Elbow;
		private Axis ax_Wrist;
		private Axis ax_Gripper;
		
		private DateTime dt_LastSend;
		
		//Const Arm Info
		public const double TableHeight = 0d;
		public const double BaseRadius = 0d;
		public const double BaseHeight = 66.56d;
		public const double Humerus = 113.28d;
		public const double Ulna = 113.28d;
		public const double Hand = 148.04d;
		
		//Const Neutral angle.
		public const double ShoulderNeutralAngle = 90;
		public const double ElbowNeutralAngle = -90;
		public const double WristNeutralAngle = 0;
		
		#endregion
		
		public RLArm () : base()
		{
			this.ax_Base = new Axis(1, "Base", -90, 90);
			this.ax_Base.PositionUpdated += this.HandleAxisUpdated;
			
			this.ax_Shoulder = new Axis(2, "Shoulder", -90, 90);
			this.ax_Shoulder.PositionUpdated += this.HandleAxisUpdated;
			
			this.ax_Elbow = new Axis(3, "Elbow", -90, 90);
			this.ax_Elbow.PositionUpdated += this.HandleAxisUpdated;
			
			this.ax_Wrist = new Axis(4, "Wrist", -90, 90);
			this.ax_Wrist.PositionUpdated += this.HandleAxisUpdated;
			
			this.ax_Gripper	= new Axis(5, "Gripper", -90, 90);
			this.ax_Gripper.PositionUpdated += this.HandleAxisUpdated;
			
			this.ls_Axis = new List<Axis>(new Axis[] {
				this.ax_Base,
				this.ax_Shoulder,
				this.ax_Elbow,
				this.ax_Wrist,
				this.ax_Gripper
			});
			
			this.sepo_ComPort = new SerialPort();
			this.dt_LastSend = DateTime.Now;
			
			//Widgets
			this.ls_View = new List<Gtk.Widget>(new Gtk.Widget[] {
				new ArmTopView(this),
				new ArmFrontView(this)
			});
			
			this.adi_AdditionalInfoWidget = new AddInfo(this);
			
			//use gripper but sould be any axis.
			this.HandleAxisUpdated(this.ax_Gripper);
		}
		
		#region Properties
		
		/// <summary>
		/// Wrist angle relative to the table.
		/// </summary>
		public double WristTable { get; private set; }
		
		public double X { get; private set; }
		
		public double Y { get; private set; }
		
		public double Z { get; private set; }
		
		#endregion
		
		public void SetXYZ(double XVal, double YVal, double ZVal, double WristTableVal)
		{
			//Inverse Kinematic Tcp to axis.
			
			double FingersLength = Math.Sqrt(Math.Pow(XVal, 2) + Math.Pow(ZVal, 2));
			
			double WristLength = (FingersLength - (Math.Cos(Radians(WristTableVal)) * RLArm.Hand)) - RLArm.BaseRadius;
			double WristHeight = (YVal - (Math.Sin(Radians(WristTable)) * RLArm.Hand)) - (RLArm.TableHeight + RLArm.BaseHeight);
			
			double SEWOpening = Math.Sqrt(Math.Pow(WristLength, 2) + Math.Pow(WristHeight, 2));
			double SEWOpeningPercent = SEWOpening / (RLArm.Humerus + RLArm.Ulna);
			
			double A1 = Math.Atan2(WristHeight, WristLength);
			double A2 = Math.Acos((Math.Pow(RLArm.Humerus, 2) - Math.Pow(RLArm.Ulna, 2) + Math.Pow(SEWOpening, 2)) / ((2 * RLArm.Humerus) * SEWOpening));
			
			double BasePosition = Degrees(Math.Atan2(ZVal, XVal));
			double ShoulderPosition = Degrees(A1 + A2);
			double ElbowPosition = -(180 - Degrees(Math.Acos((Math.Pow(RLArm.Humerus, 2) + Math.Pow(RLArm.Ulna, 2) - Math.Pow(SEWOpening, 2)) / ((2 * RLArm.Humerus) * RLArm.Ulna))));
			double WristPosition = WristTable - (ShoulderPosition + ElbowPosition);

			ShoulderPosition -= ShoulderNeutralAngle;
			ElbowPosition -= ElbowNeutralAngle;
			WristPosition -= WristNeutralAngle;
			
			Console.WriteLine("IK Base: {0}, Elbow: {1}, Shoulder:{2}, Wrist: {3}, X: {4}, Y:{5}, Z:{6}, WristTable: {7}",
			                  BasePosition, ElbowPosition,
			                  ShoulderPosition, WristPosition,
			                  XVal, YVal, ZVal, WristTableVal);
			
			//TODO log out of range to console
			if(SEWOpeningPercent > 1)
				return;
			
			try {
				//Use local so to not trigger update too many times.
				this.ax_Base.Position = BasePosition;
				this.ax_Elbow.Position = ElbowPosition;
				this.ax_Shoulder.Position = ShoulderPosition;
				this.ax_Wrist.Position = WristPosition;
			} catch (ArgumentOutOfRangeException) {
				//TODO log out of range to console
				Console.WriteLine("IK Out of range");
			}
		}
		
		/// <summary>
		/// Handle a value change from an axis.
		/// </summary>
		/// <param name="Sender">
		/// A <see cref="Axis"/>
		/// </param>
		private void HandleAxisUpdated(Axis Sender)
		{
			//Kinematic from Axis to Tcp
			
			//Calc X Y
			double ShoulderLength = RLArm.BaseRadius;
			double ShoulderHeight = RLArm.TableHeight + BaseHeight;
			
			double CurrentAngle = this.ax_Shoulder.Position + RLArm.ShoulderNeutralAngle;
			
			double ElbowLength = Math.Cos(Radians(CurrentAngle)) * RLArm.Humerus + ShoulderLength;
			double ElbowHeight = Math.Sin(Radians(CurrentAngle)) * RLArm.Humerus + ShoulderHeight;
			
			CurrentAngle += this.ax_Elbow.Position + ElbowNeutralAngle;
			
			double WristLength = Math.Cos(Radians(CurrentAngle)) * RLArm.Ulna + ElbowLength;
			double WristHeight = Math.Sin(Radians(CurrentAngle)) * RLArm.Ulna + ElbowHeight;
			
			CurrentAngle += this.ax_Wrist.Position + RLArm.WristNeutralAngle;
			
			double FingersLength = Math.Cos(Radians(CurrentAngle)) * RLArm.Hand + WristLength;
			double FingersHeight = Math.Sin(Radians(CurrentAngle)) * RLArm.Hand + WristHeight;
			
			//Calc Z
			double FingersX = Math.Cos(Radians(this.ax_Base.Position)) * FingersLength;
			double FingersZ = Math.Sin(Radians(this.ax_Base.Position)) * FingersLength;
			
			//Use private value so to not trigger HandleXYZChanged.
			this.X = FingersX;
			this.Y = FingersHeight;
			this.Z = FingersZ;
			this.WristTable = CurrentAngle;
			
			Console.WriteLine("Axis Base: {0}, Elbow: {1}, Shoulder:{2}, Wrist: {3}, X: {4}, Y:{5}, Z:{6}, WristTable: {7}",
			                  this.ax_Base.Position, this.ax_Elbow.Position,
			                  this.ax_Shoulder.Position, this.ax_Wrist.Position,
			                  this.X, this.Y, this.Z, this.WristTable);
			
			//Axis updated, update graphics.
			RunGtkThread.RunOnGtkThread(()=>{
				foreach (Gtk.Widget GtkItem in this.ls_View) {
					GtkItem.QueueDraw();
				}
			});
			
			//Only need to update the axis that changed
			this.OnPositionUpdated(Sender);
		}
		
		private void OnPositionUpdated(Axis Sender)
		{
			if(this.sepo_ComPort.IsOpen && this.dt_LastSend.AddMilliseconds(50) < DateTime.Now) {
				//Start sending update
				
				//a<axisNum><angle>s
				string Cmd = string.Format("a{0}{1}s", Sender.Index, (int)(Sender.Position + 90));
				Console.WriteLine(Cmd);
				
				byte[] Data = System.Text.Encoding.ASCII.GetBytes(Cmd);
				this.sepo_ComPort.Write(Data, 0, Data.Length);
				
				this.dt_LastSend = DateTime.Now;
				
				//End
			}
			
			if(this.PositionUpdated != null)
				this.PositionUpdated(this);
		}
		
		/// <summary>
		/// List of all axis.
		/// </summary>
		public override List<Axis> AxisList {
			get { return this.ls_Axis; }
		}
		
		/// <summary>
		/// Window to setup the arm.
		/// </summary>
		public override Gtk.Window ArmSetup {
			get
			{
				if(this.pswin_PortSettingWindow == null)
					this.pswin_PortSettingWindow = new PortSetting(this.sepo_ComPort);
				return this.pswin_PortSettingWindow;
			}
		}
		
		/// <summary>
		/// List of view of the arm.
		/// </summary>
		public override List<Gtk.Widget> Views {
			get { return this.ls_View; }
		}
		
		/// <summary>
		/// A widget displaying all the additional info of the device.
		/// </summary>
		public override Gtk.Widget AdditionalInfoWidget {
			get { return this.adi_AdditionalInfoWidget; }
		}
		
		public override string Header()
		{
			System.Text.StringBuilder Builder = new System.Text.StringBuilder();
			
			Builder.AppendLine("ArmController TmpArm;");
			Builder.Append("if (!ArmController.ControllerContext.TryGetValue(new Guid(\"");
			Builder.Append(this.Id.ToString());
			Builder.AppendLine("\"), out TmpArm))");
			Builder.AppendLine("	throw new KeyNotFoundException(\"Arm id not found\");");
			Builder.AppendLine("armcontrolgui.RandomLogicArm.RLArm Arm = (armcontrolgui.RandomLogicArm.RLArm)TmpArm;");
			
			return Builder.ToString();
		}
		
		public static double Radians(double Degrees)
		{
			return Degrees * (Math.PI / 180);
		}
		
		public static double Degrees(double Radians)
		{
			return Radians * (180 / Math.PI);
		}
	}
}

